/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    GaitScheduler.cpp
 * @author
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include "common/Controllers/GaitScheduler.h"
#include <iostream>

/*!
 * @file GaitScheduler.cpp
 * @brief Logic for fixed-gait timing
 */

/*=========================== Gait Data ===============================*/
/**
 * Reset gait data to zeros
 */
template <typename T>
void GaitData<T>::zero()
{
  // Stop any gait transitions
  _nextGait = _currentGait;

  // General Gait descriptors
  periodTimeNominal = 0.0;     // 总周期
  initialPhase = 0.0;          // 初始相位为0
  switchingPhaseNominal = 0.0; // 转换触点的法相位
  overrideable = 0;            // allows the gait parameters to be overridden

  // Enable flag for each foot
  gaitEnabled = Eigen::Vector4i::Zero(); // enable gait controlled legs

  // Time based descriptors
  periodTime = Vec4<T>::Zero();           // 整体步态周期时间
  timeStance = Vec4<T>::Zero();           // 总站姿时间
  timeSwing = Vec4<T>::Zero();            // 总摆动时间
  timeStanceRemaining = Vec4<T>::Zero();  // 站姿剩余时间
  timeSwingRemaining = Vec4<T>::Zero();   // 剩余摆动时间

  // Phase based descriptors
  switchingPhase = Vec4<T>::Zero();  // 切换至摆动的相位
  phaseVariable = Vec4<T>::Zero();   // 每只脚的整体步态阶段
  phaseOffset = Vec4<T>::Zero();     // 发步态相位设置为0
  phaseScale = Vec4<T>::Zero();      // 相对于变量的相位标度
  phaseStance = Vec4<T>::Zero();     // 站姿亚相
  phaseSwing = Vec4<T>::Zero();      // 摆动副相

  // Scheduled contact states
  contactStateScheduled = Eigen::Vector4i::Zero(); // contact state of the foot脚的接触状态
  contactStatePrev = Eigen::Vector4i::Zero();      // previous contact state of the foot脚以前的接触状态
  touchdownScheduled = Eigen::Vector4i::Zero();    // scheduled touchdown flag预定着陆标志
  liftoffScheduled = Eigen::Vector4i::Zero();      // scheduled liftoff flag预定升空标志
}

template struct GaitData<double>;
template struct GaitData<float>;

/*========================= Gait Scheduler ============================*/

/**
 * Constructor to automatically setup a basic gait
 */
template <typename T>
GaitScheduler<T>::GaitScheduler(UserParameters *_userParameters, float _dt)
{
  initialize();
  userParameters = _userParameters;
  dt = _dt;
}

/**
 * Initialize the gait data
 */
template <typename T>
void GaitScheduler<T>::initialize()
{
  std::cout << "[GAIT] Initialize Gait Scheduler" << std::endl;

  // Start the gait in a trot since we use this the most
  //开始步态类型设置为站立状态，因为我们用这个最多
  gaitData._currentGait = GaitType::STAND;

  // Zero all gait data将所有步态数据归零
  gaitData.zero();

  // Create the gait from the nominal initial从标称初始值创建步态
  createGait();
  period_time_natural = gaitData.periodTimeNominal;
  switching_phase_natural = gaitData.switchingPhaseNominal;
}

/**
 * Executes the Gait Schedule step to calculate values for the defining
 * gait parameters.
 */
template <typename T>
void GaitScheduler<T>::step()
{
  // Modify the gait with settings
  //初始化步态参数
  modifyGait();

  if (gaitData._currentGait != GaitType::STAND)
  {
    // Track the reference phase variable
    gaitData.initialPhase = fmod((gaitData.initialPhase + (dt / gaitData.periodTimeNominal)), 1);
  }

  // Iterate over the feet
  for (int foot = 0; foot < 4; foot++)
  {
    // Set the previous contact state for the next timestep
    // * 由于这是规划，所以规划的pre状态由上一次规划结果得到
    gaitData.contactStatePrev(foot) = gaitData.contactStateScheduled(foot);

    if (gaitData.gaitEnabled(foot) == 1)
    {
      // Monotonic time based phase incrementation
      if (gaitData._currentGait == GaitType::STAND)
      {
        // Don't increment the phase when in stand mode
        // 在待机模式下不要增加相位
        dphase = 0.0;
      }
      else
      {
        //这条是基于周期相位变化
        //公式：该腿的相位=该腿的相位刻度*（迭代周期/总周期时间）
        dphase = gaitData.phaseScale(foot) * (dt / gaitData.periodTimeNominal);
      }

      // Find each foot's current phase
      //找到每只脚的当前相位
      //公式：每只脚的当前相位=上一次相位+本次的相位增量
      gaitData.phaseVariable(foot) =
          fmod((gaitData.phaseVariable(foot) + dphase), 1);

      //计算支撑/摆动腿状态相关参数

      // Check the current contact state
      /*若处于支撑相阶段，计算支撑向步态相关参数*/
      if (gaitData.phaseVariable(foot) <= gaitData.switchingPhase(foot))
      {
        // Foot is scheduled to be in contact
        //设置足部接触状态：支撑相=1，摆动相=0
        gaitData.contactStateScheduled(foot) = 1;

        // Stance subphase calculation
        //支撑相状态=每只脚的当前相位【0~1】/相切换到swing的相位【1】
        gaitData.phaseStance(foot) =
            gaitData.phaseVariable(foot) / gaitData.switchingPhase(foot);

        // Swing phase has not started since foot is in stance
        //计算摆动相位，摆动阶段尚未开始，因为脚处于站姿
        gaitData.phaseSwing(foot) = 0.0;

        // Calculate the remaining time in stance
        //站姿剩余的时间=步态总时间*（摆动相位-现在的相位）
        gaitData.timeStanceRemaining(foot) =
            gaitData.periodTime(foot) *
            (gaitData.switchingPhase(foot) - gaitData.phaseVariable(foot));

        // Foot is in stance, no swing time remaining
        //计算摆动剩余时间，支撑相没有摆动时间
        gaitData.timeSwingRemaining(foot) = 0.0;

        // First contact signifies scheduled touchdown
        //判断是不是第一次接触，处理从站立到其他步态刚开始逻辑用的
        if (gaitData.contactStatePrev(foot) == 0)
        {
          // Set the touchdown flag to 1
          gaitData.touchdownScheduled(foot) = 1;
        }
        else
        {
          // Set the touchdown flag to 0
          gaitData.touchdownScheduled(foot) = 0;
        }
      }
       /*若处于摆动相阶段，计算支撑向步态相关参数*/
      else
      {
        // Foot is not scheduled to be in contact
        //设置足部接触状态：支撑相=1，摆动相=0，摆动相足部没有接触
        gaitData.contactStateScheduled(foot) = 0;

        // Stance phase has completed since foot is in swing
        gaitData.phaseStance(foot) = 1.0;

        // Swing subphase calculation
        // 摆动相相位=（当前的相位-摆动相默认相位）/（1-摆动相默认相位）
        gaitData.phaseSwing(foot) =
            (gaitData.phaseVariable(foot) - gaitData.switchingPhase(foot)) /
            (1.0 - gaitData.switchingPhase(foot));

        // Foot is in swing, no stance time remaining
        //计算站姿剩余时间：脚在摆动，没有站立时间，故站立剩余时间为0
        gaitData.timeStanceRemaining(foot) = 0.0;

        // Calculate the remaining time in swing
        //摆动态剩余时间=步态总时间*（1-当前相位值）
        gaitData.timeSwingRemaining(foot) =
            gaitData.periodTime(foot) * (1 - gaitData.phaseVariable(foot));

        // First contact signifies scheduled touchdown
        //判断是不是第一次摆动，设置摆动标志位，处理从站立到其他步态刚开始逻辑用的
        if (gaitData.contactStatePrev(foot) == 1)
        {
          // Set the liftoff flag to 1
          gaitData.liftoffScheduled(foot) = 1;
        }
        else
        {
          // Set the liftoff flag to 0
          gaitData.liftoffScheduled(foot) = 0;
        }
      }
    }
     /*如果腿未启用步态调度器*/
    else
    {
      // Leg is not enabled
      gaitData.phaseVariable(foot) = 0.0;

      // Foot is not scheduled to be in contact
      gaitData.contactStateScheduled(foot) = 0;
    }
  }
}

/**
 * @brief 根据用户参数列表生成新的步态
 * @tparam T
 */
template <typename T>
void GaitScheduler<T>::modifyGait()
{
  // Choose the gaits and parameters as selected
  switch ((int)userParameters->gait_override)
  {
  case 0:
    // Use default gait and default settings from Control code
    if (gaitData._currentGait != gaitData._nextGait)
    {
      createGait();
    }
    break;

  case 1:
    // Use chosen gait with default settings
    if (gaitData._currentGait != GaitType(userParameters->gait_type))
    {
      gaitData._nextGait = GaitType(userParameters->gait_type);
      createGait();
    }
    break;

  case 2:
    // Use chosen gait and chosen settings
    // Change the gait
    if (gaitData._currentGait != GaitType(userParameters->gait_type))
    {
      gaitData._nextGait = GaitType(userParameters->gait_type);
      createGait();
    }

    // Adjust the gait parameters
    if (fabs(gaitData.periodTimeNominal - (T)userParameters->gait_period_time) > 0.0001 ||
        fabs(gaitData.switchingPhaseNominal - (T)userParameters->gait_switching_phase) > 0.0001)
    {
      // Modify the gait with new parameters if it is overrideable
      if (gaitData.overrideable == 1)
      {
        gaitData.periodTimeNominal = userParameters->gait_period_time;
        gaitData.switchingPhaseNominal = userParameters->gait_switching_phase;
        calcAuxiliaryGaitData();
      }
    }
    break;

  case 3:
    // Use NaturalGaitModification from FSM_State
    if (gaitData._currentGait != gaitData._nextGait)
    {
      createGait();
    }
    break;

  case 4:
    // Use NaturalGaitModification from FSM_State
    if (gaitData._currentGait != gaitData._nextGait)
    {
      createGait();
      period_time_natural = gaitData.periodTimeNominal;
      switching_phase_natural = gaitData.switchingPhaseNominal;
    }
    else
    {
      gaitData.periodTimeNominal = period_time_natural;
      gaitData.switchingPhaseNominal = switching_phase_natural;
      calcAuxiliaryGaitData();
    }
    break;
  }
}

/**
 *功能：根据每个步态的类型和参数创建步态结构函数
 *要创建标准步态，只需定义以下内容：
 *   gaitData.periodTimeNominal
 *   gaitData.switchingPhaseNominal
 *   gaitData.phaseOffset
 * 其余设置为：
 *   gaitData.gaitEnabled << 1, 1, 1, 1;
 *   gaitData.initialPhase = 0.0;
 *   gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
 *
 *这些增加了灵活性，用于非常不规则的步态和过渡。
 */
template <typename T>
void GaitScheduler<T>::createGait()
{
  std::cout << "[GAIT] Transitioning gait from " << gaitData.gaitName
            << " to ";
  // Case structure gets the appropriate parameters
  switch (gaitData._nextGait)
  {
  case GaitType::STAND:
    gaitData.gaitName = "STAND";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 10.0;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 1.0;
    gaitData.phaseOffset << 0.5, 0.5, 0.5, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 0;
    break;

  case GaitType::STAND_CYCLE:
    gaitData.gaitName = "STAND_CYCLE";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 1.0;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 1.0;
    gaitData.phaseOffset << 0.5, 0.5, 0.5, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 0;
    break;

  case GaitType::STATIC_WALK:
    gaitData.gaitName = "STATIC_WALK";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 1;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.75;
    gaitData.phaseOffset << 0.25, 0.0, 0.75, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::AMBLE:
    gaitData.gaitName = "AMBLE";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.5;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.6250;
    gaitData.phaseOffset << 0.0, 0.5, 0.25, 0.75;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::TROT_WALK:
    gaitData.gaitName = "TROT_Turn";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.45;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.55;
    gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::TROT:
    gaitData.gaitName = "TROT";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.45;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.55;
    gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::TROT_RUN:
    gaitData.gaitName = "TROT_RUN";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.4;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.4;
    gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::PACE:
    gaitData.gaitName = "PACE";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.35;
    gaitData.initialPhase = 0.25;
    gaitData.switchingPhaseNominal = 0.5;
    gaitData.phaseOffset << 0.0, 0.5, 0.0, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::BOUND:
    gaitData.gaitName = "BOUND";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.4;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.4;
    gaitData.phaseOffset << 0.0, 0.0, 0.5, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::ROTARY_GALLOP:
    gaitData.gaitName = "ROTARY_GALLOP";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.4;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.2;
    gaitData.phaseOffset << 0.0, 0.8571, 0.3571, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::TRAVERSE_GALLOP:
    // TODO: find the right sequence, should be easy
    gaitData.gaitName = "TRAVERSE_GALLOP";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.5;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.2;
    gaitData.phaseOffset << 0.0, 0.8571, 0.3571, 0.5;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::PRONK:
    gaitData.gaitName = "PRONK";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.5;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.5;
    gaitData.phaseOffset << 0.0, 0.0, 0.0, 0.0;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::THREE_FOOT:
    gaitData.gaitName = "THREE_FOOT";
    gaitData.gaitEnabled << 0, 1, 1, 1;
    gaitData.periodTimeNominal = 0.4;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.666;
    gaitData.phaseOffset << 0.0, 0.666, 0.0, 0.333;
    gaitData.phaseScale << 0.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 1;
    break;

  case GaitType::CUSTOM:
    gaitData.gaitName = "CUSTOM";
    // TODO: get custom gait parameters from operator GUI
    break;

  case GaitType::JUMP:
    gaitData.gaitName = "JUMP";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    gaitData.periodTimeNominal = 0.5;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal = 0.2;
    gaitData.phaseOffset << 0.0, 0.0, 0.0, 0.0;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 0;
    break;

  case GaitType::TRANSITION_TO_STAND:
    gaitData.gaitName = "TRANSITION_TO_STAND";
    gaitData.gaitEnabled << 1, 1, 1, 1;
    T oldGaitPeriodTimeNominal = gaitData.periodTimeNominal;
    gaitData.periodTimeNominal = 3 * gaitData.periodTimeNominal;
    gaitData.initialPhase = 0.0;
    gaitData.switchingPhaseNominal =
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.switchingPhaseNominal - 1)) / gaitData.periodTimeNominal;
    gaitData.phaseOffset << (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(0) - 1)) / gaitData.periodTimeNominal,
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(1) - 1)) / gaitData.periodTimeNominal,
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(2) - 1)) / gaitData.periodTimeNominal,
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(3) - 1)) / gaitData.periodTimeNominal;
    gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
    gaitData.overrideable = 0;
    break;
  }

  // Gait has switched
  gaitData._currentGait = gaitData._nextGait;

  std::cout << gaitData.gaitName << "\n"
            << std::endl;

  // Calculate the auxilliary gait information
  //根据上面的7个步态类型参数为每只脚设置步态参数
  calcAuxiliaryGaitData();
}

/**
 * @brief 根据指定的参数计算步态数据
 *
 * @tparam T
 */
template <typename T>
void GaitScheduler<T>::calcAuxiliaryGaitData()
{

  // Set the gait parameters for each foot
  for (int foot = 0; foot < 4; foot++)
  {
    if (gaitData.gaitEnabled(foot) == 1)
    {
      // The scaled period time for each foot
      //计算周期时间，周期时间=步态周期 /腿的相位范围
      gaitData.periodTime(foot) =
          gaitData.periodTimeNominal / gaitData.phaseScale(foot);

      // Phase at which to switch the foot from stance to swing
      //将脚从站姿切换到摆动的阶段
      gaitData.switchingPhase(foot) = gaitData.switchingPhaseNominal;

      // Initialize the phase variables according to offset
      //计算偏移量初始化相位变量，偏移量初始化相位变量=腿的初始相位+相位偏移
      gaitData.phaseVariable(foot) =
          gaitData.initialPhase + gaitData.phaseOffset(foot);

      // Find the total stance time over the gait cycle
      //计算整个步态周期的总站姿时间，整个步态周期的总站姿时间=步态周期*站立在步态周期的比例
      gaitData.timeStance(foot) =
          gaitData.periodTime(foot) * gaitData.switchingPhase(foot);

      // Find the total swing time over the gait cycle
      //计算整个步态周期的总摆动时间
      gaitData.timeSwing(foot) =
          gaitData.periodTime(foot) * (1.0 - gaitData.switchingPhase(foot));
    }
    //若步态没有使能
    else
    {
      // The scaled period time for each foot
      gaitData.periodTime(foot) = 0.0;

      // Phase at which to switch the foot from stance to swing
      gaitData.switchingPhase(foot) = 0.0;

      // Initialize the phase variables according to offset
      gaitData.phaseVariable(foot) = 0.0;

      // Foot is never in stance
      gaitData.timeStance(foot) = 0.0;

      // Foot is always in "swing"
      gaitData.timeSwing(foot) = 1.0 / gaitData.periodTime(foot);
    }
  }
}

/**
 * Prints relevant information about the gait and current gait state
 */
template <typename T>
void GaitScheduler<T>::printGaitInfo()
{
  // Increment printing iteration
  printIter++;

  // Print at requested frequency
  if (printIter == printNum)
  {
    std::cout << "[GAIT SCHEDULER] Printing Gait Info...\n";
    std::cout << "Gait Type: " << gaitData.gaitName << "\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Enabled: " << gaitData.gaitEnabled(0) << " | "
              << gaitData.gaitEnabled(1) << " | " << gaitData.gaitEnabled(2)
              << " | " << gaitData.gaitEnabled(3) << "\n";
    std::cout << "Period Time: " << gaitData.periodTime(0) << "s | "
              << gaitData.periodTime(1) << "s | " << gaitData.periodTime(2)
              << "s | " << gaitData.periodTime(3) << "s\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Contact State: " << gaitData.contactStateScheduled(0) << " | "
              << gaitData.contactStateScheduled(1) << " | "
              << gaitData.contactStateScheduled(2) << " | "
              << gaitData.contactStateScheduled(3) << "\n";
    std::cout << "Phase Variable: " << gaitData.phaseVariable(0) << " | "
              << gaitData.phaseVariable(1) << " | " << gaitData.phaseVariable(2)
              << " | " << gaitData.phaseVariable(3) << "\n";
    std::cout << "Stance Time Remaining: " << gaitData.timeStanceRemaining(0)
              << "s | " << gaitData.timeStanceRemaining(1) << "s | "
              << gaitData.timeStanceRemaining(2) << "s | "
              << gaitData.timeStanceRemaining(3) << "s\n";
    std::cout << "Swing Time Remaining: " << gaitData.timeSwingRemaining(0)
              << "s | " << gaitData.timeSwingRemaining(1) << "s | "
              << gaitData.timeSwingRemaining(2) << "s | "
              << gaitData.timeSwingRemaining(3) << "s\n";
    std::cout << std::endl;

    // Reset iteration counter
    printIter = 0;
  }
}

template class GaitScheduler<double>;
template class GaitScheduler<float>;

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
